#ifndef REGISTRATION_H
#define REGISTRATION_H

#include <pcl/io/pcd_io.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/search/kdtree.h>

/*!
 \file registration.h This file contains the registration functions.
 \author Sqizzato Stefano
 \author Maroso Alessandro

*/

using namespace std;
using namespace pcl;

/*!
 \brief Performs a registration based on uniform keypoints, FPFH and correspondences filtered with SAC.

 \param[in] sourceColor Cloud to register: not modified after computation.
 \param[in] targetColor Reference cloud.
 \param[out] registeredColor Output of the computation.
*/
void registerSourceToTarget (PointCloud<PointXYZRGB>::Ptr sourceColor , PointCloud<PointXYZRGB>::Ptr targetColor, PointCloud<PointXYZRGB>::Ptr registeredColor);

/*!
 \brief Performs a registration using Iterative Closest Point on a downsampled copy of the source.

 \param[in] source Cloud to register: not modified after computation.
 \param[in] target Reference cloud.
 \param[out] registered Output of the computation.
 \param[out] final_transformation_matrix The 4x4 matrix representing the rototranslation.
 \param[in] resolution Leafsize of downsampled cloud on which ICP runs.
*/
void finalRegisterSourceToTarget (PointCloud<PointXYZ>::Ptr source, PointCloud<PointXYZ>::Ptr target, PointCloud<PointXYZ>::Ptr registered, Eigen::Matrix4f &final_transformation_matrix, float resolution);

/*!
 \brief This function returns a score based on squared distance between target and source cloud.

 \param[in] source The point cloud to give a score.
 \param[in] target The reference point cloud from which distances are computed.
 \return float The score (mean square distances): lower is better.
*/
template< class PointT >
float validationScoreProximityTarget (PointCloud<PointT> &source, PointCloud<PointT> &target)
{
    boost::shared_ptr< PointCloud<PointT> > sourcePtr = source.makeShared();
    search::KdTree<PointT> tree;
    tree.setInputCloud(sourcePtr);
    float sum=0;
    for (int i=0;i<target.size();i++)
    {
        PointT currentPoint=target.at(i);
        vector<int> indices;
        vector<float> sqrDist;
        tree.nearestKSearch(currentPoint,1,indices,sqrDist);
        sum+=sqrDist.at(0);
    }
    return sum/target.size();
}

#endif
